// 定义硬件Timer1寄存器（UNO引脚9）
#define SERVO_PWM_PIN 9

void setup() {
  // 配置16位Timer1为相位修正PWM模式（50Hz）
  TCCR1A = _BV(COM1A1) | _BV(WGM11);
  TCCR1B = _BV(WGM13) | _BV(CS11);
  ICR1 = 39999;  // 16MHz/8/50Hz-1
  DDRB |= _BV(PB1);  // 设置D9为输出

  // 初始化中立位（1.5ms）
  OCR1A = 3000;  // 3000 = (1.5ms/20ms)*40000
}

void loop() {
  // 平滑扫描示例（带加速度限制）
  static float pos = 90, target = 0, speed = 0;
  const float max_speed = 0.5;  // °/ms
  
  // 目标位置交替变化
  if(abs(pos - target) < 1.0) target = (target == 0) ? 180 : 0;
  
  // 速度规划（梯形加速度）
  float accel = (target > pos) ? 0.02 : -0.02;
  speed = constrain(speed + accel, -max_speed, max_speed);
  pos = constrain(pos + speed, 0, 180);
  
  // 更新PWM（硬件级操作）
  uint16_t pulse = map(pos*10, 0, 1800, 1000, 5000);
  OCR1A = pulse;
  
  delay(10);  // 控制周期10ms
}